package com.awesumgames.awesum;

import static java.lang.Math.sqrt;
import static java.lang.Math.sin;
import static java.lang.Math.cos;
import static java.lang.Math.asin;
import static java.lang.Math.acos;
import static java.lang.Math.atan2;

import java.util.Locale;

import javax.vecmath.Quat4f;

import android.opengl.Matrix;

import com.awesumgames.awesum.Vector3;

import static com.awesumgames.awesum.AwesumGlobal.Awesum.radToDeg;
import static com.awesumgames.awesum.AwesumGlobal.Awesum.degToRad;

public class Quaternion {
	public float w, x, y, z;
	private static final float TOLERANCE = 0.00001f;

	public Quaternion() {
		w = 1f;
		x = y = z = 0f;
	}
	public Quaternion(float w, float x, float y, float z) {
		this.w = w; this.x = x; this.y = y; this.z = z;
	}
	public Quaternion(float w, Vector3 v) {
		this.w = w; x = v.x; y = v.y; z = v.z;
	}
	public Quaternion(Quaternion q2) {
		set(q2);
	}
	
	public String toString() {
		return String.format(Locale.US, "(%f, %f, %f, %f)", w, x, y, z);
	}

	public Quaternion set(Quaternion q2) {
		w = q2.w; x = q2.x; y = q2.y; z = q2.z;
		return this;
	}
	public Quaternion set(Quat4f q2) {
		w = q2.w; x = q2.x; y = q2.y; z = q2.z;
		return this;
	}
	
	public Quaternion copy() {
		return new Quaternion(this);
	}
	
	public boolean isIdentity() {
		return w==1f && x==0f && y==0f && z==0f; 
	}
	
	public float magnitudeSq() {
		return w*w + x*x + y*y + z*z;
	}
	public float magnitude() {
		return (float)sqrt(magnitudeSq());
	}
	public Quaternion normalize() {
		/*float magSq = magnitudeSq();
		float absMagSq = abs(magSq);
		
		if (absMagSq > TOLERANCE && absMagSq - 1f > TOLERANCE) {
			float mag = (float)sqrt(magSq);
			w = clamp(w / mag, -1f, 1f);
			x /= mag; y /= mag; z /= mag;
		}*/
		float mag = magnitude();
		if (mag > 0.0f) {
			x /= mag;
			y /= mag;
			z /= mag;
			w /= mag;
		} else {
			x = y = z = 0f;
			w = 1f;
		}
		
		return this;
	}
	public Quaternion conjugate() {
		return new Quaternion(w, -x, -y, -z);
	}
	public Quaternion multiply(Quaternion q2) {
		float rw = w * q2.w - x * q2.x - y * q2.y - z * q2.z;
		float rx = w * q2.x + x * q2.w + y * q2.z - z * q2.y;
		float ry = w * q2.y + y * q2.w + z * q2.x - x * q2.z;
		float rz = w * q2.z + z * q2.w + x * q2.y - y * q2.x;
		w = rw; x = rx; y = ry; z = rz;
		return this;
	}
	public Quaternion rotateLoc(Quaternion q2) { //local rotation
		return multiply(q2);
	}
	public Quaternion rotateEnv(Quaternion q2) { //environmental rotation
		return this.set(q2.copy().multiply(this));
	}
	public Quaternion rotateLocXRad(float a) {return multiply(new Quaternion().setEuler(a, 0f, 0f));}
	public Quaternion rotateLocXDeg(float a) {return rotateLocXRad(a * degToRad);}
	public Quaternion rotateLocYRad(float a) {return multiply(new Quaternion().setEuler(0f, a, 0f));}
	public Quaternion rotateLocYDeg(float a) {return rotateLocYRad(a * degToRad);}
	public Quaternion rotateLocZRad(float a) {return multiply(new Quaternion().setEuler(0f, 0f, a));}
	public Quaternion rotateLocZDeg(float a) {return rotateLocZRad(a * degToRad);}
	public Quaternion rotateEnvXRad(float a) {return rotateEnv(new Quaternion().setEuler(a, 0f, 0f));}
	public Quaternion rotateEnvXDeg(float a) {return rotateEnvXRad(a * degToRad);}
	public Quaternion rotateEnvYRad(float a) {return rotateEnv(new Quaternion().setEuler(0f, a, 0f));}
	public Quaternion rotateEnvYDeg(float a) {return rotateEnvYRad(a * degToRad);}
	public Quaternion rotateEnvZRad(float a) {return rotateEnv(new Quaternion().setEuler(0f, 0f, a));}
	public Quaternion rotateEnvZDeg(float a) {return rotateEnvZRad(a * degToRad);}
	
	public Vector3 rotate(Vector3 v) {
		Quaternion q2 = copy().multiply(new Quaternion(0f, v.x, v.y, v.z)).multiply(conjugate()); //v.z, v.x, v.y));
		return new Vector3(q2.x, q2.y, q2.z); //q2.y, q2.z, q2.x);
	}

	public Quaternion setEuler(Vector3 v) {
		return setEuler(v.x, v.y, v.z);
	}
	public Quaternion setEuler(float ex, float ey, float ez) {
		float fr = ex / 2f; //theta			//py
		float fp = ey / 2f; //psi			//yr
		float fy = ez / 2f; //phi			//rp
		double	sinp = sin(fp),	siny = sin(fy),	sinr = sin(fr),
				cosp = cos(fp),	cosy = cos(fy),	cosr = cos(fr);
		w = (float)(cosr * cosp * cosy + sinr * sinp * siny);
		x = (float)(sinr * cosp * cosy - cosr * sinp * siny);
		y = (float)(cosr * sinp * cosy + sinr * cosp * siny);
		z = (float)(cosr * cosp * siny - sinr * sinp * cosy);
		
		return normalize();
	}
	public Quaternion setEulerDeg(Vector3 v) {
		return setEuler(v.copy().multiply(degToRad));
	}
	public Quaternion setEulerDeg(float ex, float ey, float ez) {
		return setEuler(ex * degToRad, ey * degToRad, ez * degToRad);
	}

	public Vector3 getEuler() { //rotate in order Y, Z, X
		normalize();
		/*float theta = (float)asin(clamp(2f * (w * y - z * x), -1f, 1f)); //x
		float psi = (float)atan2(2f * (w * z + x * y), 1f - 2f * (y*y + z*z)); //y
		float phi = (float)atan2(2f * (w * x + y * z), 1f - 2f * (x*x + y*y)); //z
		return new Vector3(theta, psi, phi);*/
		float roll, pitch, yaw;
		float test = x * y + z * w;
		if (test > 0.499f) { // singularity at north pole
			pitch = 2f * (float)atan2(x, w);
			yaw = (float)Math.PI / 2f;
			roll = 0f;
			return new Vector3(roll, pitch, yaw);
		} else if (test < -0.499f) { // singularity at south pole
			pitch = -2f * (float)atan2(x, w);
			yaw = -(float)Math.PI / 2f;
			roll = 0f;
			return new Vector3(roll, pitch, yaw);
		}
		float sqx = x * x, sqy = y * y, sqz = z * z;
		pitch = (float)atan2(2f * y * w - 2f * x * z, 1f - 2f * sqy - 2f * sqz);
		yaw = (float)asin(2f * test);
		roll = (float)atan2(2f * x * w - 2f * y * z, 1f - 2f * sqx - 2f * sqz);
		return new Vector3(roll, pitch, yaw);
	}
	public Vector3 getEulerDeg() {
		return getEuler().multiply(radToDeg);
	}
	
	public Vector3 axis() {
		//float scale = (float)Math.sqrt(x * x + y * y + z * z);
		//return new Vector3(y / scale, z / scale, x / scale);
		Vector3 axis = new Vector3(x, y, z);
		float mag = axis.magnitude();
		if (mag > TOLERANCE)
			axis.divide(mag);
		if ((float)acos(w) <= (float)Math.PI / 2f)
			return axis;
		return axis.negate2();
	}
	public float angle() {
		//return (float)Math.acos(w) * 2f;
		float angle = (float)acos(w) * 2f;
        return angle <= (float)Math.PI ? angle : 2f * (float)Math.PI - angle;
	}
	public float angleDeg() {
		return angle() * radToDeg;
	}
	public Quaternion setAxisAngle(Vector3 axis, float angle) {
		float mag = axis.magnitude();
		if (mag < TOLERANCE) { //null rotation
			w = 1f;
			x = y = z = 0f;
		} else {
			angle /= 2f;
			Vector3 n = axis.copy().normalize2();
			float sinAngle = (float)sin(angle);
			w = (float)cos(angle);
			x = sinAngle * n.x / mag;
			y = sinAngle * n.y / mag;
			z = sinAngle * n.z / mag;
		}
		return this;
	}
	public Quaternion setAxisAngleDeg(Vector3 axis, float angle) {
		return setAxisAngle(axis, angle * degToRad);
	}
	
	public float[] getMatrix() {
		float ww = w*w, xx = x*x, yy = y*y, zz = z*z;
	    float xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
		float invs = 1 / (xx + yy + zz + ww); //inverse square length--only required if quat isn't already normalized

		return new float[] {
			(xx - yy - zz + ww) * invs,	2f * (xy + zw) * invs,		2f * (xz - yw) * invs,		0,
			2f * (xy - zw) * invs,		(-xx + yy - zz + ww) * invs,2f * (yz + xw) * invs,		0,
			2f * (xz + yw) * invs,		2f * (yz - xw) * invs,		(-xx - yy + zz + ww) * invs,0,
			0, 0, 0, 1
		};
	}
}
